#ifndef __position_solutions__
#define __position_solutions__

#include <vector>
#include <boost/numeric/ublas/matrix.hpp>
#include <cmath>
#include "point.hpp"

/**
   robot dimensions
   in milimeters
*/
const double PI=M_PI; //or any other value if M_PI not presented :)
const double d2=149.09,
  d4= 433.07,
  d6= 56.25,
  a2= 431.80,
  a3= 20.32;

double work_sphere_r();
     
double dead_cylinder_r();

enum modifier{Minus=-1,Plus=1};

using namespace boost::numeric;

typedef boost::numeric::ublas::matrix<double> matr;

typedef std::vector< ublas::matrix<double> > matrix_vector;

typedef ublas::vector<double> uvector;

double g2r(double);
double r2g(double);


inline matr identity()
{
  return matr(ublas::identity_matrix<double>(4));
}


matr special_inverse(matr T);

point to_angles(const point &O, const point &D, double t=0);

matr fit_orient(const point &o, const point &d, double t=0);



matrix_vector
forward_solution_A( double q1,
		    double q2,
		    double q3,
		    double q4,
		    double q5,
		    double q6);

matrix_vector
forward_solution_T( double q1,
		    double q2,
		    double q3,
		    double q4,
		    double q5,
		    double q6,
		    matr T0=ublas::identity_matrix<double>(4) );

matrix_vector
forward_solution_T( std::vector<double> Q,
		    matr T0=ublas::identity_matrix<double>(4));



std::vector<double>
reverse_solution(matr T,
		 modifier k1,
		 modifier k2,
		 modifier W,
		 matr T0=ublas::identity_matrix<double>(4));


std::vector<ublas::vector<double> >
quantise_line(ublas::vector<double> a,
	      ublas::vector<double> b,
	      double delta);

matrix_vector
quantise_translate(matr A, matr B, double delta);

matr Rotx(double phi);
matr Roty(double phi);
matr Rotz(double phi);

matr oat2R(double o, double a, double t);
matr oat2R(ublas::vector<double> a);

matr build_trans(double x, double y, double z,
		 double o, double a, double t);


matr build_trans(point p);

matr build_trans(std::vector<double> P);


ublas::vector<double> R2oat(matr T);
std::vector<double> T2xyz(matr T);

point T2pnt(matr T);

point point_dir(const point &p);

     

#endif
